#include "lidar_det.hpp"

void lio_odom_callback(const nav_msgs::Odometry::Ptr & current_msg)
{
    current_point.x= current_msg->pose.pose.position.x;
    current_point.y= current_msg->pose.pose.position.y;
    current_point.z= current_msg->pose.pose.position.z;
    

}
void local_odom_callback(const nav_msgs::Odometry::Ptr & current_msg)
{
    current_angle.x= current_msg->pose.pose.orientation.x;
    current_angle.y= current_msg->pose.pose.orientation.y;
    current_angle.z= current_msg->pose.pose.orientation.z;
    current_angle.w= current_msg->pose.pose.orientation.w;
    curr_plane_angle = unionsys_core->toEulerAngle(current_angle);
    current_angle_Q = Eigen::Quaterniond(current_msg->pose.pose.orientation.w, current_msg->pose.pose.orientation.x,
                                        current_msg->pose.pose.orientation.y, current_msg->pose.pose.orientation.z);
}


void laser_map_callback(const sensor_msgs::PointCloud2::ConstPtr &drone_cloud)
{
    pcl::PointCloud<pcl::PointXYZ> laser_map;
    pcl::fromROSMsg(*drone_cloud, laser_map);
    if (laser_map.points.size() != laser_map.width * laser_map.height) {
        std::cout<<"====>error points222.."<<std::endl;
    }
    laser_map.width = laser_map.points.size();
    laser_map.height = 1;
    // if(laser_map.points.size()>0&&ros::Time::now().toSec()-map_save_last>5.0)
    // {
    //     pcl::io::savePCDFileASCII ("/home/orangepi/map_saved/map_als.pcd", laser_map);//保存pcd
    //     map_save_last = ros::Time::now().toSec();
    // }
}

void human_callback(const swarm_msgs::BoundingBoxes &msg)
{
    std::vector<Box> receive_boxes = deal_detection_to_3dpose(msg, "human", 1.8, 0);
    if(receive_boxes.size()>0)
    {
        double min_dist_vehicle2box = 5;
        for(int i=0;i<receive_boxes.size();i++)
        {
            Eigen::Vector3d receive_center;
            receive_center(0) = (receive_boxes[i].x_min+receive_boxes[i].x_max)/2.0;
            receive_center(1) = (receive_boxes[i].y_min+receive_boxes[i].y_max)/2.0;
            receive_center(2) = (receive_boxes[i].z_min+receive_boxes[i].z_max)/2.0;
            double dist_tmp = sqrt((receive_center(0)-current_point.x)*(receive_center(0)-current_point.x)+
                                    (receive_center(1)-current_point.y)*(receive_center(1)-current_point.y)+
                                    (receive_center(2)-current_point.z)*(receive_center(2)-current_point.z));
            if(dist_tmp<min_dist_vehicle2box)
            {
                geometry_msgs::Point pose_temp;
                pose_temp.x = receive_center(0);
                pose_temp.y = receive_center(1);
                pose_temp.z = receive_center(2);
                int have_near_target = 0;
                for(int j=0;j<human_list.size();j++)
                {
                    double dist_two_target = sqrt((receive_center(0)-human_list[j].x)*(receive_center(0)-human_list[j].x)+
                                                (receive_center(1)-human_list[j].y)*(receive_center(1)-human_list[j].y)+
                                                (receive_center(2)-human_list[j].z)*(receive_center(2)-human_list[j].z));
                    if(dist_two_target<1.0)
                    {
                        human_list[j] = pose_temp;
                        have_near_target++;
                        break;
                    }
                }
                if(have_near_target==0)
                {
                    human_list.push_back(pose_temp);
                }
                
                
            }
        }  
    }
}

void car_callback(const swarm_msgs::BoundingBoxes &msg)
{
    std::vector<Box> receive_boxes = deal_detection_to_3dpose(msg, "car", 1.0, 50);
    if(receive_boxes.size()>0)
    {
        double min_dist_vehicle2box = 5;
        geometry_msgs::Point pose_nearest;
        for(int i=0;i<receive_boxes.size();i++)
        {
            Eigen::Vector3d receive_center;
            receive_center(0) = (receive_boxes[i].x_min+receive_boxes[i].x_max)/2.0;
            receive_center(1) = (receive_boxes[i].y_min+receive_boxes[i].y_max)/2.0;
            receive_center(2) = (receive_boxes[i].z_min+receive_boxes[i].z_max)/2.0;
            double dist_tmp = sqrt((receive_center(0)-current_point.x)*(receive_center(0)-current_point.x)+
                                    (receive_center(1)-current_point.y)*(receive_center(1)-current_point.y)+
                                    (receive_center(2)-current_point.z)*(receive_center(2)-current_point.z));
            if(dist_tmp<min_dist_vehicle2box)
            {
                min_dist_vehicle2box = dist_tmp;
                pose_nearest.x = receive_center(0);
                pose_nearest.y = receive_center(1);
                pose_nearest.z = receive_center(2);    
            }
        }  
        car_pose_pub.publish(pose_nearest);
    }
}

void oil_callback(const swarm_msgs::BoundingBoxes &msg)
{
    std::vector<Box> receive_boxes = deal_detection_to_3dpose(msg, "oil", 1.0, 100);
    if(receive_boxes.size()>0)
    {
        double min_dist_vehicle2box = 5;
        for(int i=0;i<receive_boxes.size();i++)
        {
            Eigen::Vector3d receive_center;
            receive_center(0) = (receive_boxes[i].x_min+receive_boxes[i].x_max)/2.0;
            receive_center(1) = (receive_boxes[i].y_min+receive_boxes[i].y_max)/2.0;
            receive_center(2) = (receive_boxes[i].z_min+receive_boxes[i].z_max)/2.0;
            double dist_tmp = sqrt((receive_center(0)-current_point.x)*(receive_center(0)-current_point.x)+
                                    (receive_center(1)-current_point.y)*(receive_center(1)-current_point.y)+
                                    (receive_center(2)-current_point.z)*(receive_center(2)-current_point.z));
            if(dist_tmp<min_dist_vehicle2box)
            {
                geometry_msgs::Point pose_temp;
                pose_temp.x = receive_center(0);
                pose_temp.y = receive_center(1);
                pose_temp.z = receive_center(2);
                int have_near_target = 0;
                for(int j=0;j<oil_list.size();j++)
                {
                    double dist_two_target = sqrt((receive_center(0)-oil_list[j].x)*(receive_center(0)-oil_list[j].x)+
                                                (receive_center(1)-oil_list[j].y)*(receive_center(1)-oil_list[j].y)+
                                                (receive_center(2)-oil_list[j].z)*(receive_center(2)-oil_list[j].z));
                    if(dist_two_target<1.0)
                    {
                        oil_list[j] = pose_temp;
                        have_near_target++;
                        break;
                    }
                }
                if(have_near_target==0)
                {
                    oil_list.push_back(pose_temp);
                }
                
                
            }
        }  
    }
}


void weapon_callback(const swarm_msgs::BoundingBoxes &msg)
{
    std::vector<Box> receive_boxes = deal_detection_to_3dpose(msg, "weapon", 1.4, 150);
    if(receive_boxes.size()>0)
    {
        double min_dist_vehicle2box = 5;
        for(int i=0;i<receive_boxes.size();i++)
        {
            Eigen::Vector3d receive_center;
            receive_center(0) = (receive_boxes[i].x_min+receive_boxes[i].x_max)/2.0;
            receive_center(1) = (receive_boxes[i].y_min+receive_boxes[i].y_max)/2.0;
            receive_center(2) = (receive_boxes[i].z_min+receive_boxes[i].z_max)/2.0;
            double dist_tmp = sqrt((receive_center(0)-current_point.x)*(receive_center(0)-current_point.x)+
                                    (receive_center(1)-current_point.y)*(receive_center(1)-current_point.y)+
                                    (receive_center(2)-current_point.z)*(receive_center(2)-current_point.z));
            if(dist_tmp<min_dist_vehicle2box)
            {
                geometry_msgs::Point pose_temp;
                pose_temp.x = receive_center(0);
                pose_temp.y = receive_center(1);
                pose_temp.z = receive_center(2);
                int have_near_target = 0;
                for(int j=0;j<weapon_list.size();j++)
                {
                    double dist_two_target = sqrt((receive_center(0)-weapon_list[j].x)*(receive_center(0)-weapon_list[j].x)+
                                                (receive_center(1)-weapon_list[j].y)*(receive_center(1)-weapon_list[j].y)+
                                                (receive_center(2)-weapon_list[j].z)*(receive_center(2)-weapon_list[j].z));
                    if(dist_two_target<1.0)
                    {
                        weapon_list[j] = pose_temp;
                        have_near_target++;
                        break;
                    }
                }
                if(have_near_target==0)
                {
                    weapon_list.push_back(pose_temp);
                }
                
                
            }
        }  
    }
}

void bigarmor_callback(const swarm_msgs::BoundingBoxes &msg)
{
    std::vector<Box> receive_boxes = deal_detection_to_3dpose(msg, "bigarmor", 4.0, 200);
    if(receive_boxes.size()>0)
    {
        double min_dist_vehicle2box = 5;
        for(int i=0;i<receive_boxes.size();i++)
        {
            Eigen::Vector3d receive_center;
            receive_center(0) = (receive_boxes[i].x_min+receive_boxes[i].x_max)/2.0;
            receive_center(1) = (receive_boxes[i].y_min+receive_boxes[i].y_max)/2.0;
            receive_center(2) = (receive_boxes[i].z_min+receive_boxes[i].z_max)/2.0;
            double dist_tmp = sqrt((receive_center(0)-current_point.x)*(receive_center(0)-current_point.x)+
                                    (receive_center(1)-current_point.y)*(receive_center(1)-current_point.y)+
                                    (receive_center(2)-current_point.z)*(receive_center(2)-current_point.z));
            if(dist_tmp<min_dist_vehicle2box)
            {
                geometry_msgs::Point pose_temp;
                pose_temp.x = receive_center(0);
                pose_temp.y = receive_center(1);
                pose_temp.z = receive_center(2);
                int have_near_target = 0;
                for(int j=0;j<bigarmor_list.size();j++)
                {
                    double dist_two_target = sqrt((receive_center(0)-bigarmor_list[j].x)*(receive_center(0)-bigarmor_list[j].x)+
                                                (receive_center(1)-bigarmor_list[j].y)*(receive_center(1)-bigarmor_list[j].y)+
                                                (receive_center(2)-bigarmor_list[j].z)*(receive_center(2)-bigarmor_list[j].z));
                    if(dist_two_target<1.0)
                    {
                        bigarmor_list[j] = pose_temp;
                        have_near_target++;
                        break;
                    }
                }
                if(have_near_target==0)
                {
                    bigarmor_list.push_back(pose_temp);
                }
                
                
            }
        }  
    }
}

void shatanche_callback(const swarm_msgs::BoundingBoxes &msg)
{
    std::vector<Box> receive_boxes = deal_detection_to_3dpose(msg, "shatanche", 2.0, 250);
    if(receive_boxes.size()>0)
    {
        double min_dist_vehicle2box = 5;
        for(int i=0;i<receive_boxes.size();i++)
        {
            Eigen::Vector3d receive_center;
            receive_center(0) = (receive_boxes[i].x_min+receive_boxes[i].x_max)/2.0;
            receive_center(1) = (receive_boxes[i].y_min+receive_boxes[i].y_max)/2.0;
            receive_center(2) = (receive_boxes[i].z_min+receive_boxes[i].z_max)/2.0;
            double dist_tmp = sqrt((receive_center(0)-current_point.x)*(receive_center(0)-current_point.x)+
                                    (receive_center(1)-current_point.y)*(receive_center(1)-current_point.y)+
                                    (receive_center(2)-current_point.z)*(receive_center(2)-current_point.z));
            if(dist_tmp<min_dist_vehicle2box)
            {
                geometry_msgs::Point pose_temp;
                pose_temp.x = receive_center(0);
                pose_temp.y = receive_center(1);
                pose_temp.z = receive_center(2);
                int have_near_target = 0;
                for(int j=0;j<shatanche_list.size();j++)
                {
                    double dist_two_target = sqrt((receive_center(0)-shatanche_list[j].x)*(receive_center(0)-shatanche_list[j].x)+
                                                (receive_center(1)-shatanche_list[j].y)*(receive_center(1)-shatanche_list[j].y)+
                                                (receive_center(2)-shatanche_list[j].z)*(receive_center(2)-shatanche_list[j].z));
                    if(dist_two_target<1.0)
                    {
                        shatanche_list[j] = pose_temp;
                        have_near_target++;
                        break;
                    }
                }
                if(have_near_target==0)
                {
                    shatanche_list.push_back(pose_temp);
                }
                
                
            }
        }  
    }
}



void publish_target_callback(const ros::TimerEvent& /*event*/) 
{
    if(human_list.size()>0)
    {
        human_pose_pub.publish(human_list[pub_human_cnt]);
        pub_human_cnt++;
        if(pub_human_cnt==human_list.size())
        {
            pub_human_cnt=0;
        }
    }
    // if(car_list.size()>0)
    // {
    //     car_pose_pub.publish(car_list[pub_car_cnt]);
    //     pub_car_cnt++;
    //     if(pub_car_cnt==car_list.size())
    //     {
    //         pub_car_cnt=0;
    //     }
    // }
    if(oil_list.size()>0)
    {
        oil_pose_pub.publish(oil_list[pub_oil_cnt]);
        pub_oil_cnt++;
        if(pub_oil_cnt==oil_list.size())
        {
            pub_oil_cnt=0;
        }
    }
    if(weapon_list.size()>0)
    {
        weapon_pose_pub.publish(weapon_list[pub_weapon_cnt]);
        pub_weapon_cnt++;
        if(pub_weapon_cnt==weapon_list.size())
        {
            pub_weapon_cnt=0;
        }
    }
    if(bigarmor_list.size()>0)
    {
        bigarmor_pose_pub.publish(bigarmor_list[pub_bigarmor_cnt]);
        pub_bigarmor_cnt++;
        if(pub_bigarmor_cnt==bigarmor_list.size())
        {
            pub_bigarmor_cnt=0;
        }
    }
    if(shatanche_list.size()>0)
    {
        shatanche_pose_pub.publish(shatanche_list[pub_shatanche_cnt]);
        pub_shatanche_cnt++;
        if(pub_shatanche_cnt==shatanche_list.size())
        {
            pub_shatanche_cnt=0;
        }
    }
}


int main(int argc, char **argv)
{
    ros::init(argc, argv, "lidar_detect");
    ros::NodeHandle nh;
    unionsys_core->detect_serial_number();

    //read parameters from launch file

    nh.param<int>("image_width",image_width,1024);
    nh.param<int>("image_height",image_height,768);
    
    //Subscriber
    local_odom_sub = nh.subscribe("/mavros/local_position/odom",10,local_odom_callback);
    LIO_odom_sub = nh.subscribe("/Odometry",10,lio_odom_callback);

    laser_map_sub = nh.subscribe("/Laser_map",10,laser_map_callback);

    human_yolo_sub = nh.subscribe("/bian/human",10,human_callback);
    car_yolo_sub = nh.subscribe("/bian/car",10,car_callback);
    oil_yolo_sub = nh.subscribe("/bian/oil",10,oil_callback);
    weapon_yolo_sub = nh.subscribe("/bian/weapon",10,weapon_callback);
    bigarmor_yolo_sub = nh.subscribe("/bian/bigarmor",10,bigarmor_callback);
    shatanche_yolo_sub = nh.subscribe("/bian/shatanche",10,shatanche_callback);

    

    //Publisher
    marker_pub = nh.advertise<visualization_msgs::Marker>("/kdtree_box", 1);
    marker_id_pub = nh.advertise<visualization_msgs::Marker>("/kdtree_id_marker", 1);

    human_pose_pub = nh.advertise<geometry_msgs::Point>("/zhou/pose_human", 10);
    car_pose_pub = nh.advertise<geometry_msgs::Point>("/zhou/pose_car", 10);
    oil_pose_pub = nh.advertise<geometry_msgs::Point>("/zhou/pose_oil", 10);
    weapon_pose_pub = nh.advertise<geometry_msgs::Point>("/zhou/pose_weapon", 10);

    bigarmor_pose_pub = nh.advertise<geometry_msgs::Point>("/zhou/pose_bigarmor", 10);
    shatanche_pose_pub = nh.advertise<geometry_msgs::Point>("/zhou/pose_shatanche", 10);

    //Timer
    publish_target_timer = nh.createTimer(ros::Duration(0.01), publish_target_callback);

    //the setpoint publishing rate MUST be faster than 2Hz
    ros::Rate rate(20);
    //main loop here
    while(ros::ok()){
        ros::spinOnce();
        rate.sleep();
    }
    return 0;
}

std::vector<Box> deal_detection_to_3dpose(swarm_msgs::BoundingBoxes msg, std::string class_name, double real_size,int id_zone)
{
    std::vector<Box> box_queue_return;
    if(msg.bounding_boxes.size()>0)
    {
        int clusterId_draw = id_zone;
        for(int i=0;i<msg.bounding_boxes.size();i++)
        {
            clusterId_draw++;

            double newx1 = (image_width/2.0)-(msg.bounding_boxes[i].xmax+msg.bounding_boxes[i].xmin)/2.0 ; // 15 pixel bias width
            double newy1 = (msg.bounding_boxes[i].ymax+msg.bounding_boxes[i].ymin)/2.0-(image_height/2.0);
            double center_angle = atan2(newy1, newx1);  //left rotation is + ,diverse it            
            // std::cout<<"----center_angle: "<<center_angle*rad2deg<<std::endl;

            double dist_to_camera_center = sqrt(newx1*newx1+newy1*newy1);

            //nihe
            double pitch_deg = 90.0-dist_to_camera_center*0.375;
            // std::cout<<"====>pitch_deg: "<<pitch_deg<<std::endl; 
            double pitch_to_camera = pitch_deg*deg2rad;
            //calc box size
            double yolo_box_size = sqrt((msg.bounding_boxes[i].xmax-msg.bounding_boxes[i].xmin)*(msg.bounding_boxes[i].xmax-msg.bounding_boxes[i].xmin)
                                        +(msg.bounding_boxes[i].ymax-msg.bounding_boxes[i].ymin)*(msg.bounding_boxes[i].ymax-msg.bounding_boxes[i].ymin));
            // std::cout<<"----yolo_box_size: "<<yolo_box_size<<std::endl;
            double virtual_focus = 176.0;

            double real_dist = (real_size*virtual_focus)/yolo_box_size;
            // std::cout<<"------->real distance : "<<real_dist<<std::endl;
            if(real_dist>10.0)
            {
                continue;
            }
            
            Eigen::Vector3d vector_ray;
            vector_ray(0) = real_dist*cos(pitch_to_camera)/sqrt(1+tan(center_angle)*tan(center_angle));
            vector_ray(1) = real_dist*cos(pitch_to_camera)*tan(center_angle)/sqrt(1+tan(center_angle)*tan(center_angle));
            vector_ray(2) = -real_dist*sin(pitch_to_camera);
            if(abs(center_angle)>90.0*deg2rad)
            {
                vector_ray(0) = -vector_ray(0);
                vector_ray(1) = -vector_ray(1);
            }
            // std::cout<<"--->camera raw pos : "<<vector_ray(0)<<","<<vector_ray(1)<<","<<vector_ray(2)<<std::endl;
            Eigen::Matrix3d Rpg_now = current_angle_Q.toRotationMatrix();

            Eigen::Vector3d vector_trans = Rpg_now*vector_ray;

            vector_trans(0) += current_point.x;
            vector_trans(1) += current_point.y;
            vector_trans(2) += current_point.z;

            // std::cout<<"--->camera predict pos : "<<vector_trans(0)<<","<<vector_trans(1)<<","<<vector_trans(2)<<std::endl;
            Box vision_box;
            vision_box.x_min = vector_trans(0)-real_size/2.0;
            vision_box.y_min = vector_trans(1)-real_size/2.0;
            vision_box.z_min = vector_trans(2)-real_size/2.0;
            vision_box.x_max = vector_trans(0)+real_size/2.0;
            vision_box.y_max = vector_trans(1)+real_size/2.0;
            vision_box.z_max = vector_trans(2)+real_size/2.0;
            vision_box.id = 0;
            draw_box(vision_box, clusterId_draw, 0.2, 0, 0.5, 0.5);
            draw_name(vision_box, class_name, clusterId_draw);
            box_queue_return.push_back(vision_box);

        }
    }
    return box_queue_return;
}


void draw_box(Box temp_box, int id, double width, double red, double green, double blue)
{
    //draw box on rviz
    visualization_msgs::Marker line_strip;
    line_strip.header.frame_id = "map";
    line_strip.header.stamp = ros::Time::now();
    line_strip.ns = "target_box";//namespace
    line_strip.action = visualization_msgs::Marker::ADD;
    line_strip.pose.orientation.w = 1.0;
    line_strip.id = id; //unique id, useful when multiple markers exist.
    line_strip.type = visualization_msgs::Marker::LINE_STRIP; //marker type
    line_strip.lifetime = ros::Duration(0.5);
    line_strip.scale.x = width; //width of the line
    line_strip.color.r = red; 
    line_strip.color.g = green; 
    line_strip.color.b = blue;
    line_strip.color.a = 1.0; //alpha ,set 1
    geometry_msgs:: Point p[8];
    p[0].x = temp_box.x_min;     p[0].y = temp_box.y_max;     p[0].z = temp_box.z_max;
    p[1].x = temp_box.x_min;     p[1].y = temp_box.y_min;     p[1].z = temp_box.z_max;
    p[2].x = temp_box.x_min;     p[2].y = temp_box.y_min;     p[2].z = temp_box.z_min;
    p[3].x = temp_box.x_min;     p[3].y = temp_box.y_max;     p[3].z = temp_box.z_min;
    p[4].x = temp_box.x_max;     p[4].y = temp_box.y_max;     p[4].z = temp_box.z_min;
    p[5].x = temp_box.x_max;     p[5].y = temp_box.y_min;     p[5].z = temp_box.z_min;
    p[6].x = temp_box.x_max;     p[6].y = temp_box.y_min;     p[6].z = temp_box.z_max;
    p[7].x = temp_box.x_max;     p[7].y = temp_box.y_max;     p[7].z = temp_box.z_max;
    //LINE_STRIP link 0-1 1-2 2-3 only
    for (int i = 0; i < 8; i++){
        line_strip.points.push_back(p[i]);
    }
    //so we need to add every links mannualy
    line_strip.points.push_back(p[0]);
    line_strip.points.push_back(p[3]);
    line_strip.points.push_back(p[2]);
    line_strip.points.push_back(p[5]);
    line_strip.points.push_back(p[6]);
    line_strip.points.push_back(p[1]);
    line_strip.points.push_back(p[0]);
    line_strip.points.push_back(p[7]);
    line_strip.points.push_back(p[4]);
    marker_pub.publish(line_strip);
    
    //end draw

    
}

void draw_name(Box temp_box,std::string name, int id)
{
    geometry_msgs::PoseStamped box_center;
    box_center.pose.position.x = (temp_box.x_min+temp_box.x_max)/2.0;
    box_center.pose.position.y = (temp_box.y_min+temp_box.y_max)/2.0;
    box_center.pose.position.z = (temp_box.z_min+temp_box.z_max)/2.0;

    //draw target name on box
    visualization_msgs::Marker marker_id;
    marker_id.header.frame_id="map";
    marker_id.header.stamp = ros::Time::now();
    marker_id.ns = "basic_shapes";
    marker_id.action = visualization_msgs::Marker::ADD;
    marker_id.pose.orientation.w = 1.0;
    marker_id.id =id;
    marker_id.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
    marker_id.lifetime = ros::Duration(0.5);
    marker_id.scale.z = 0.3;
    marker_id.color.b = 0;
    marker_id.color.g = 0;
    marker_id.color.r = 255;
    marker_id.color.a = 1;
    geometry_msgs::Pose name_pose;
    name_pose.position.x =  box_center.pose.position.x;
    name_pose.position.y =  box_center.pose.position.y;
    name_pose.position.z =  temp_box.z_max;
    marker_id.text= name;
    marker_id.pose= name_pose;
    marker_id_pub.publish(marker_id);
}